#include <wiringPi.h>
#include <softPwm.h>
#include <iostream>
#include <cmath>
#include "motor.h"

using namespace std;

MOTOR::MOTOR() {
}

void MOTOR::speedSet(int pwm) {
    // cout << "motor pwm \t" << pwm << endl;
    if(pwm >= 0) {
        if (pwm > MAX_SPEED) pwm = MAX_SPEED ;

        softPwmWrite(MOTOR_PIN_PLUS, pwm);
        softPwmWrite(MOTOR_PIN_MINUS, 0);

    } else {
        softPwmWrite(MOTOR_PIN_PLUS, 0);
        softPwmWrite(MOTOR_PIN_MINUS, 0);
    }
    // else {
    //     if (pwm < -MAX_SPEED) pwm = -MAX_SPEED ;

    //     softPwmWrite(MOTOR_PIN_MINUS, -pwm);
    //     softPwmWrite(MOTOR_PIN_PLUS, 0);
    // }
}

// 讲GPIO25初始化为输入引脚，兼容马达驱动
void MOTOR::init() {
    softPwmCreate(MOTOR_PIN_PLUS, 0, 100);
    softPwmCreate(MOTOR_PIN_MINUS, 0, 100);

    softPwmWrite(MOTOR_PIN_PLUS, 0);
    softPwmWrite(MOTOR_PIN_MINUS, 0);
    // pinMode(MOTOR_COMPATABLE, INPUT);
}
